#ifndef OOPSMP_UTILS_GEOMETRY_WORKSPACE_ARTICULATEDPART_H_
#define OOPSMP_UTILS_GEOMETRY_WORKSPACE_ARTICULATEDPART_H_

#include "OOPSMP/Utils/Geometry/Workspace/Part.H"
#include "OOPSMP/Utils/Geometry/Workspace/Material.H"
#include "OOPSMP/Utils/DataStructures/ChainTree/ChainTree.h"
#include "OOPSMP/Utils/Geometry/Joints/Joints.h"

#include "OOPSMP/Utils/Math/Algebra/vector3.h"
#include "OOPSMP/Utils/Math/Algebra/matrix44.h"

#include <queue>
#include <vector>
#include <cstring>
#include <assert.h>

namespace OOPSMP
{
	const double EPSILON = ldexp(1.0, -20);

	/*
	 *@brief returns true iff two groups are equal
	 *@param elta first group
	 *@param eltb second group
	 *@param gSize groups size
	 */
	static inline bool groupApproxCompare(const double * elta, const double * eltb, const int gSize)
	{
		for(int i = 0; i < gSize; i++)
			if(fabs(elta[i] - eltb[i]) > EPSILON)
				return false;
		return true;
	}

	/**
     *@brief Macro for class, class pointer, and const class pointer declarations
     */
	ForwardClassDeclaration(ArticulatedPart);
	
	/**
     *@brief Geometry of workspace articulated objects
     */
	class ArticulatedPart : public Part {

	public:
		
		/**
		 *@brief Articulated part constructor
		 */
		ArticulatedPart() : OOPSMP::Part(), n_totalNrTriangles( 0 ), n_totalNrVertices( 0 ), triangles( NULL ) {}
		
		/**
		 *@brief Articulated part desstructor
		 */
		~ArticulatedPart() {
			if (triangles != NULL) {
				delete [] triangles;
			}
		}
		
		/**
         *@brief Sets bbox class used for this articulated part
		 *@param strType name of bbox class
		 */
		void setBBoxContainerType( const char * const strType ) {
			ct.setBBoxType( strType );
		}
		
		/**
         *@brief Defines size of articulated part
		 *@param size number of physical links (robot links)
		 */
		void setNrLinks( const int num ) { ct.initCTSize(num); }
		
		/**
         *@brief Returns number of physical links in this articulated part
		 */
		int getLinkCount() const { return ct.getLinkCount(); }
		
		/**
         *@brief Adds physical link to articulated part
		 *@param lnk reference to added link
		 */
		void addLink( CTLink_t lnk ) {
			ct.addLink( lnk );
		}
		
		/**
         *@brief Adds physical link to articulated part, given part and joint
		 *@param part pointer to part
		 *@param j pointer to joint
		 */
		void addLink( Part_t part, Joints_t joint ) {
			addLink( new CTLink( part, joint ) );
		}
		
		/**
         *@brief Returns constant pointer to part associated with link
		 *@param i link index in chain tree
		 */
		Part_const_t getLinkPart( const int i ) const {
			return ct.getNode(i).getLink().getPart();
		}
		
		/**
         *@brief Returns index of root in chain tree
		 */
		const int getRoodId() const { return ct.getRootId(); }

		/**
         *@brief Completes setup of robot
		 */
		void completeSetup() {

			ct.completeChainTree();

			stateVectorLength = 0;
			svIndexItem item;

			for ( long i = 0 ; i < ct.getLinkCount() ; i++ ) {
				// build stateVectorIndex
				int dofs = ct.getNode(i).getLink().getJoint()->getNumDoFs();

				item.pos = stateVectorLength; item.size = dofs;
				v_stateVectorIndex.push_back( item );

				stateVectorLength += dofs;
			}

			state_vector = (double *)calloc( stateVectorLength, sizeof(double) );
			updateBoundingBoxes();

			calcTotalTriangles();
			calcTotalVertices();

			triangles = new int[ n_totalNrTriangles ];
			for ( int i = 0 ; i < n_totalNrTriangles ; i++ ) {
				triangles[i] = v_allTriangles.at(i);
			}

		}
		
		/**
         *@brief Computes all chain tree bboxes, given state
		 *@param state current state of articulated part
		 */
		void updateBoundingBoxes() {

			// declare and init auxiliary matrices and vectors
			OOPSMPmatrix44_t trans = OOPSMPalloc_matrix44();
			OOPSMPidentity_matrix44(trans);
			OOPSMPmatrix44_t trans2 = OOPSMPalloc_matrix44();
			OOPSMPmatrix44_t transtmp = OOPSMPalloc_matrix44();
			OOPSMPvector3_t linkDimsA = OOPSMPalloc_vector3();
			OOPSMPvector3_t linkDimsB = OOPSMPalloc_vector3();
			assert( linkDimsA != NULL && linkDimsB != NULL );

			for ( unsigned i = 0 ; i < v_stateVectorIndex.size() ; i++ ) {

				// if the node isn't affected by the transformation, skip it.
				// if ( !ct.getNode(i).getLink().getIsAffected() ) continue;

				int pos = v_stateVectorIndex[i].pos;

				if ( i != 0 )
					ct.getNode(i-1).getLink().getLinkDimensions(linkDimsA);

				ct.getNode(i).getLink().getLinkDimensions(linkDimsB);

				ct.getNode(i).getLink().getJoint()->getTransformationMatrix((i == 0) ? NULL : linkDimsA, linkDimsB, state_vector + pos , transtmp);

				// multiply current state transformation matrix by the new matrix for current joint
				OOPSMPmultiply_matrix44( trans, transtmp, trans2);

				OOPSMPcopy_matrix44( trans, trans2 );
				OOPSMPzero_matrix44( trans2 );
				OOPSMPzero_matrix44( transtmp );

				// call setTransformation upon the physical part
				ct.getNodeRef(i).getLinkRef().getPart()->setTransformation( trans );

				// update the bbox of the node
				ct.computeBoundingBox( i ); 
			}

			// clean up all allocations
			OOPSMPfree_vector3( linkDimsB );
			OOPSMPfree_vector3( linkDimsA );
			OOPSMPfree_matrix44( transtmp );
			OOPSMPfree_matrix44( trans2 );
			OOPSMPfree_matrix44( trans );
		}
		
		/**
         *@brief virtual functions, must be overridden in deriving classes
		 */
		virtual void calcTotalTriangles() = 0;

		virtual void calcTotalVertices() = 0;

		/**
	     *@brief Set the articulated part transformation
	     *@param trans articulated part transformation
         */
		void setTransformation( const double * const state ) {

			// if the incoming state is the same as the current one, don't do any work
			if ( groupApproxCompare( state, state_vector, v_stateVectorIndex.back().pos + v_stateVectorIndex.back().size ) )
				return;

			// go over all nodes containing physical links, and if the node was updated, propagate the change to the root
			for ( unsigned i = 0 ; i < v_stateVectorIndex.size() ; i++ )
			{
				int pos = v_stateVectorIndex[i].pos;

				if ( !groupApproxCompare( state + pos, state_vector + pos, v_stateVectorIndex[i].size ) ) {
					ct.setIsAffected( i );
				}
			}

			// copy the new state into object's state vector
			for ( int i = 0 ; i < stateVectorLength ; i++ ) {
				state_vector[i] = state[i];
			}

			// for each physical link, update bounding boxes stored in PARTs
			updateBoundingBoxes();

		}
		
		/**
         *@brief virtual function, must be overridden in deriving classes
		 */
		virtual void getSomePointForLink( const int i, double * out ) = 0;

		/**
		 *@brief Get some point that belongs to the articulated part
		 *@param point point
         */
		void getSomePoint(double *point) const {
			ct.getNode(0).getLink().getPart()->getSomePoint( point );
		}
		
		/**
		 *@brief Get number of triangles
         */
		virtual int getNrTriangles() const	{ return n_totalNrTriangles; }
		
		/**
		 *@brief Get number of vertices
         */
		virtual int getNrVertices()	 const 	{ return n_totalNrVertices; }
		
		/**
         *@brief virtual functions, must be overridden in deriving classes
		 */
		virtual const double * getVertex(const int i) const = 0;

		virtual void getVertex(const int i, double * v) const = 0;
		
		/**
		 *@brief Get all the triangles
         */
		virtual const int* getTriangles(void) const { return triangles; }
		
		/**
		 *@brief Get the vertex indices of the i-th triangle
	     *@param i triangle index
         */
		virtual const int* getTriangle(const int i) const
		{
			assert(i >= 0 && i < getNrTriangles());

			int * m_triangle = new int[3];
			m_triangle[0] = v_allTriangles.at(3 * i + 0);
			m_triangle[1] = v_allTriangles.at(3 * i + 1);
			m_triangle[2] = v_allTriangles.at(3 * i + 2);

			return m_triangle;
		}
		
		/**
		 *@brief Get a copy of the (x, y, z) coordinates of the 3 vertices of the i-th triangle
		 *@param i triangle index
		 *@param tv0 triangle vertex
		 *@param tv1 triangle vertex
		 *@param tv2 triangle vertex
         */
		virtual void getTriangleVertices(const int i, OOPSMPvector3_t tv0, OOPSMPvector3_t tv1, OOPSMPvector3_t tv2) const
		{
			assert(tv0 != NULL);
			assert(tv1 != NULL);
			assert(tv2 != NULL);

			const int *ti = getTriangle(i);

			getVertex(ti[0], tv0);
			getVertex(ti[1], tv1);
			getVertex(ti[2], tv2);
		}
		
		/**
		 *@brief Get the (x, y, z) coordinates of the 3 vertices of the i-th triangle
	     *@param i triangle index
	     *@param tv triangle coordinates -- 9 doubles
         */
		virtual void getTriangleVertices(const int i, double *tv) const
		{
			assert(tv != NULL);	    
			getTriangleVertices(i, &(tv[0]), &(tv[3]), &(tv[6]));
		}

		/*virtual bool areInCollision(double *bbox_a, double *bbox_b) const = 0;*/

		/*bool isCollisionFree(const int nodeid, bool check_curr_level) const {

			bool found_collision = false;
			double * bboxA = new double[ct.getNode(nodeid).getLink().getBBox()->getNumElements()];
			double * bboxB = new double[ct.getNode(nodeid).getLink().getBBox()->getNumElements()];

			if (check_curr_level) // for root, always false		
			{
				CTNode slow_runner = ct.getNode(nodeid);

				while (slow_runner.getSiblingR() != -1) {
					CTNode fast_runner = slow_runner;

					bool check_collision = false;

					do {

						if (fast_runner.getLink().getIsAffected())
							check_collision = true; // we found an affected link, therefore all right brothers from here are separated from slow_runner

						fast_runner = ct.getNode(fast_runner.getSiblingR());

						if (check_collision) {

							slow_runner.getLink().getBBox()->getBBoxAsArray( bboxA ); 
							fast_runner.getLink().getBBox()->getBBoxAsArray( bboxB );

							if ( areInCollision(bboxA, bboxB) ) {
								found_collision = true;

								if (slow_runner.getChildL() == -1) // we found a collision in the bottom level
									return false;
							}
						}

					} while (fast_runner.getSiblingR() != -1);

					slow_runner = ct.getNode(slow_runner.getSiblingR());
				}

			}

			if ((found_collision) || (ct.getNode(nodeid).getLink().getIsAffected() && (ct.getNode(nodeid).getChildL() != -1)))
				// if true, we haven't reached the bottom level yet and at least one of the following is true:
				//		1. we found a collision
				//		2. the node is affected
				// either way, we need to go down one level and investigate
			{
				CTNode child_l = ct.getNode(ct.getNode(nodeid).getChildL());
				CTNode child_r = ct.getNode(ct.getNode(nodeid).getChildR());
				return (isCollisionFree(ct.getNode(nodeid).getChildL(), true) && isCollisionFree(ct.getNode(nodeid).getChildR(), false));
			}

			delete [] bboxB;
			delete [] bboxA;

			// if we reached this point, we didn't find a collision, and the tree rooted at node is ok (not affected or is a leaf)
			return true;
		}*/
		
		/**
         *@brief virtual function, must be overridden in deriving classes
		 */
		virtual bool areBBoxesInCollision(double *bbox_a, double *bbox_b) const = 0;
		
		/**
         *@brief Returns true iff the leaves of sub-trees are colliding
		 *@param nodeid_1 index of root of first sub-tree
		 *@param nodeid_2 index of root of second sub-tree
		 */
		bool areInCollision(const int nodeid_1, const int nodeid_2) const {

			CTNode node_1 = ct.getNode(nodeid_1);
			double * bboxA = new double[node_1.getLink().getBBox()->getNumElements()];
			//node_1.getLink().getBBox()->calculateBoundingBox( node_1.getLink().getPart(), bboxA );
			node_1.getLink().getBBox()->getBBoxAsArray(bboxA);

			CTNode node_2 = ct.getNode(nodeid_2);
			double * bboxB = new double[node_2.getLink().getBBox()->getNumElements()];
			//node_2.getLink().getBBox()->calculateBoundingBox( node_2.getLink().getPart(), bboxB );
			node_2.getLink().getBBox()->getBBoxAsArray(bboxB);

			if (areBBoxesInCollision(bboxA, bboxB)) {

				delete[] bboxB;
				delete[] bboxA;

				int child_1_l = node_1.getChildL();
				int child_1_r = node_1.getChildR();
				int child_2_l = node_2.getChildL();
				int child_2_r = node_2.getChildR();

				if ((child_1_l == -1) && (child_2_l == -1)) {
					// found a collision between two leaves
					return true;
				}
				/* the next two conditions hold for odd number of physical links in chain tree
				 * exactly one leaf is the direct child of the root
				 * therefore it might collide with nodes from different levels
				 */
				if (child_1_l == -1)
					return (areInCollision(nodeid_1, child_2_l) ||
							areInCollision(nodeid_1, child_2_r));

				if (child_2_l == -1)
					return (areInCollision(child_1_l, nodeid_2) ||
							areInCollision(child_1_r, nodeid_2));

				// found a collision between non-leaves
				return (areInCollision(child_1_l, child_2_l) ||
						areInCollision(child_1_l, child_2_r) ||
						areInCollision(child_1_r, child_2_l) ||
						areInCollision(child_1_r, child_2_r));
			}

			delete[] bboxB;
			delete[] bboxA;

			return false;
		}

		/**
         *@brief Returns true iff the sub-tree has no inner collisions
		 *@param nodeid index of root of sub-tree
		 */
		bool isCollisionFree(const int nodeid) const {

			CTNode node = ct.getNode(nodeid);
			int child_l_id = node.getChildL();
			
			if (child_l_id == -1) {
				// checking a leaf => no inner collisions
				return true;
			}
			
			CTNode child_l = ct.getNode(child_l_id);

			int child_r_id = node.getChildR();
			CTNode child_r = ct.getNode(child_r_id);

			if (child_r.getLink().getIsAffected()) {
				if (areInCollision(child_l_id, child_r_id) || !(isCollisionFree(child_r_id))) {
					// FILE *file = fopen("StatesLog.txt", "a");
					// fprintf(file, "failed in ArticulatedPart.h on line 319\n\n");
					// fclose(file);
					return false;
				}	
			}

			if (child_l.getLink().getIsAffected() && !(isCollisionFree(child_l_id))) {
				// FILE *file = fopen("StatesLog.txt", "a");
				// fprintf(file, "failed in ArticulatedPart.h on line 325\n\n");
				// fclose(file);
				return false;
			}

			return true;
		}
		
		/**
         *@brief Returns number of dofs for joint associated with link
		 *@param i index of link in chain tree
		 */
		int getNumDoFsForLink( const int i  ) const {
			return ct.getNode(i).getLink().getJoint()->getNumDoFs();
		}
		
		/**
         *@brief Returns constant pointer to joint associated with link
		 *@param i index of link in chain tree
		 */
		const Joints_t getLinkJoint( const int i ) const {
			return ct.getNode(i).getLink().getJoint();
		}
		
		/**
         *@brief Returns total number of all dofs of all joints (including tail)
		 */
		long getStateVectorLength() const { return stateVectorLength; }
		
		/**
         *@brief virtual function, must be overridden in deriving classes
		 */
		virtual int getPartDim() const = 0;
		
		/**
         *@brief Centers articulated part around point
		 *@param c point
		 */
		void center(const double * const c) {
			ct.getNodeRef(0).getLinkRef().getPart()->center(c);
		}

		virtual void computeBoundingBox(double *bbox) const { /* void */ }

	protected:
		
		/**
         *@brief chain tree describing articulated part
		 */
		ChainTree ct;
		
		/**
         *@brief state of articulated part
		 */
		double * state_vector;

		/**
         *@brief location info for joint in state_vector
		 *		 pos - index of starting position in state_vector
		 *		 size - number of dofs of joint
		 */
		typedef struct svIndex {
			int pos;
			int size;
		} svIndexItem;
		
		/**
         *@brief vector of location infos in state_vector
		 *		 each element represents different joint
		 */
		std::vector<svIndexItem>	v_stateVectorIndex;
		
		/**
         *@brief total number of all dofs of all joints (including tail)
		 */
		long stateVectorLength;
		
		/**
         *@brief all vertices of articulated part
		 */
		std::vector<double>			v_allVertices;
		
		/**
         *@brief all triangles of articulated part
		 */
		std::vector<int>			v_allTriangles;
		
		int * triangles;
		
		/**
         *@brief number of all triangles of articulated part
		 */
		int n_totalNrTriangles;
		
		/**
         *@brief number of all vertices of articulated part
		 */
		int n_totalNrVertices;

	};
	
	/**
     *@brief Declare factory and specify in source file the functions that should
     *       be registered for plug-and-play functionality
     */
	DeclareNoInstanceFactory(ArticulatedPart, PartFactory);

}

#endif

